#include <tinyxml.h>
#include <string>
#include <ros/ros.h>
#include <iostream>

bool LoadWaypointsFromFile(std::string inFilename)
{
    TiXmlDocument docLoad(inFilename);
    bool resLoad = docLoad.LoadFile();
    if(resLoad == false)
    {
        ROS_INFO("Failed to load waypoints... filename = %s", inFilename.c_str());
        return false;
    }

    // waterplus_map_tools::Waypoint //newWayPoint;
    TiXmlElement* RootElement = docLoad.RootElement();
    for(TiXmlNode* item = RootElement->FirstChild("Waypoint");item;item = item->NextSibling("Waypoint"))
    {
        TiXmlNode* child = item->FirstChild();
        const char* name = child->ToElement()->GetText();
        ROS_INFO("Load waypoint : %s", name);
        //newWayPoint.name = std::string(name); 
        child = item->IterateChildren(child);
        const char* pos_x = child->ToElement()->GetText();
        //newWayPoint.pose.position.x = std::atof(pos_x);
        child = item->IterateChildren(child);
        const char* pos_y = child->ToElement()->GetText();
        //newWayPoint.pose.position.y = std::atof(pos_y);
        child = item->IterateChildren(child);
        const char* pos_z = child->ToElement()->GetText();
        //newWayPoint.pose.position.z = std::atof(pos_z);
        child = item->IterateChildren(child);
        const char* ori_x = child->ToElement()->GetText();
        //newWayPoint.pose.orientation.x = std::atof(ori_x);
        child = item->IterateChildren(child);
        const char* ori_y = child->ToElement()->GetText();
        //newWayPoint.pose.orientation.y = std::atof(ori_y);
        child = item->IterateChildren(child);
        const char* ori_z = child->ToElement()->GetText();
        //newWayPoint.pose.orientation.z = std::atof(ori_z);
        child = item->IterateChildren(child);
        const char* ori_w = child->ToElement()->GetText();
        //newWayPoint.pose.orientation.w = std::atof(ori_w);
        // arWaypoint.push_back(newWayPoint);
        std::cout <<"debug cwx:"<< std::string(name)  <<std::string(pos_x)  <<std::string(pos_y)  <<std::string(pos_z)  <<std::string(ori_z)  <<std::string(ori_w)  <<std::endl;
    }
}

int main(int argc, char*argv[])
{
    /* code */
    ROS_INFO("Start load waypoint yaml!");
    LoadWaypointsFromFile("/home/wheeltec-client/waypoints.xml");
    return 0;
}

